﻿/* 
 *  <copyright file="LinearImpulseMovementCmp.cs" company="Wesley Jowitt">
 *  
 *  SobrietyEngine: Copyright (c) 2011 Wesley Jowitt
 *  
 *  This software is distributed under a software license.
 *   * As of the creation time of this file, the license in use is the Microsoft Permissive License (Ms-PL) 
 *    * The license can be viewed at:
 *     * License.Txt in the root folder of this project
 *      * http://sobrietyengine.codeplex.com/license
 *       * http://www.microsoft.com/opensource/licenses.mspx
 *       
 *  </copyright>
 *   */

using System;
using FarseerPhysics.Dynamics;
using Microsoft.Xna.Framework;
using SobrietyEngine.Geometry;


namespace SobrietyEngine.Components.Physics.Movers
{
    //TODO: apply braking at target, params to set how we behave once we hit the taget
    public class LinearImpulseMovementCmp: PhysicsMovementBase
    {

        float pushVelocity = 0.5f;
        public float PushVelocity { get { return pushVelocity; } set { pushVelocity = value; } }

        Boolean brakeAtTarget = true;
        public Boolean BrakeAtTarget { get { return brakeAtTarget; } set { brakeAtTarget = value; } }

        public LinearImpulseMovementCmp()
            : base()
        {
        }

        public LinearImpulseMovementCmp(Boolean targetlessMovement)
            : base(targetlessMovement, Vector2.Zero)
        {
        }

        public LinearImpulseMovementCmp(Boolean targetlessMovement, Vector2 dir)
            : base(targetlessMovement, dir)
        {
        }

        public override bool InitComponent()
        {
            return base.InitComponent();            
        }

        public override void Update(GameTime ElapsedTime)
        {
            if (HasTarget == false && TargetlessMovement == false)            
                return;

            //check if we are at our target
            if (HasTarget)
            {
                if (Intersect.PointPoint(posCmp.PositionV2, TargetV2, TargetRange))
                {
                    //we are at our target, select the next taget mode and get to moving
                    HasTarget = false;
                    if (BrakeAtTarget)
                        Brake();                            
                    return;
                }
            }

            //if we are moving too fast, dont apply the impulse
            //if (physCmp.Body.LinearVelocity.LengthSquared() > maxVel * maxVel)
            //    return;
            float linearVel = physCmp.Body.LinearVelocity.Length();
            float velDiff = maxVel - linearVel;
            

            //get a dir to our taret
            Vector2 dirToTarget;
            if (HasTarget)
                dirToTarget = TargetV2 - posCmp.PositionV2;
            else
                dirToTarget = Dir;

            Vector2 impulse = Vector2.Zero;

            //hit the target square on -
            if (dirToTarget.LengthSquared() < velDiff * velDiff)
            {
                impulse = dirToTarget;
            }
            //or just move at our max move velocity
            else
            {
                dirToTarget.Normalize();
                impulse = dirToTarget * velDiff;
            }

            
            //clamp velocity            
            if (ClampVelocity)
            {
                Vector2 vel = physCmp.Body.LinearVelocity;
                Vector2 clampedVel = Vector2.Clamp(physCmp.Body.LinearVelocity, maxVelocityClamp * -1, maxVelocityClamp);
                if (clampedVel.LengthSquared() < vel.LengthSquared())
                {
                    physCmp.Body.LinearVelocity = clampedVel;
                    return; //no need to apply impulse, we are already at mx
                }
            }
            
            //apply impulse
            if(physCmp.Body.BodyType == BodyType.Dynamic)
                physCmp.Body.ApplyLinearImpulse(impulse * physCmp.Body.Mass);
            else if (physCmp.Body.BodyType == BodyType.Kinematic)
            {               
                physCmp.Body.LinearVelocity = impulse;// *physCmp.Body.Mass;
            }
        }
    }
}
